#pragma once

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/float64.hpp"
#include "webots_ros2_driver/WebotsNode.hpp"

class SimDriverMi9 : public webots_ros2_driver::PluginInterface
{
    using IMUMsg = sensor_msgs::msg::Imu;
    using WheelSpeedMsg = std_msgs::msg::Float64;
    using MotorForceMsg = std_msgs::msg::Float64;
    using CameraMsg = sensor_msgs::msg::Image;

  public:
    void init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &params) override;
    void step() override;

  private:
    WbDeviceTag leftMotor;
    WbDeviceTag rightMotor;
    WbDeviceTag IMUAngle;
    WbDeviceTag IMUGyro;
    WbDeviceTag IMUAccelerometer;
    WbDeviceTag leftCamera;
    WbDeviceTag rightCamera;
    rclcpp::Publisher<IMUMsg>::SharedPtr imuPub;
    rclcpp::Publisher<WheelSpeedMsg>::SharedPtr leftWheelPub;
    rclcpp::Publisher<WheelSpeedMsg>::SharedPtr rightWheelPub;
    rclcpp::Publisher<CameraMsg>::SharedPtr leftCameraPub;
    rclcpp::Publisher<CameraMsg>::SharedPtr rightCameraPub;
    rclcpp::Subscription<MotorForceMsg>::SharedPtr leftMotorForceSub;
    rclcpp::Subscription<MotorForceMsg>::SharedPtr rightMotorForceSub;
    double leftMotorForce = 0;
    double rightMotorForce = 0;
};